#include <GlobalData.h>
#include "EQuaternion.h"

template <typename TYPE>
Quaternion<TYPE>::Quaternion()
{
	// empty
}

template <typename TYPE>
Quaternion<TYPE>::Quaternion(TYPE w, TYPE x, TYPE y, TYPE z) :
	w(w),
	x(x),
	y(y),
	z(z)
{
	// empty
}

template <typename TYPE>
Quaternion<TYPE>& Quaternion<TYPE>::normalize()
{
	// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
	// Don't normalize if we don't have to.
	TYPE mag2 = w * w + x * x + y * y + z * z;
	if (fabs(mag2) > QUATERNION_NORMALIZE_EPS && fabs(mag2 - 1.0f) > QUATERNION_NORMALIZE_EPS)
	{
		TYPE mag = sqrt(mag2);
		w /= mag;
		x /= mag;
		y /= mag;
		z /= mag;
	}
	return *this;
}

template <typename TYPE>
Quaternion<TYPE> Quaternion<TYPE>::operator-() const
{
	return Quaternion<TYPE>(-w, -x, -y, -z);
}

template <typename TYPE>
TYPE Quaternion<TYPE>::dot(const Quaternion<TYPE>& other) const
{
	return w*other.w + x*other.x + y*other.y + z*other.z;
}

template <typename TYPE>
Quaternion<TYPE> Quaternion<TYPE>::operator*(TYPE mul) const
{
	return Quaternion<TYPE>(w * mul, x * mul, y * mul, z * mul);
}
template <typename TYPE>
Quaternion<TYPE> Quaternion<TYPE>::operator/(TYPE div) const
{
	assert(div != 0.0);
	return Quaternion<TYPE>(w / div, x / div, y / div, z / div);
}

template <typename TYPE>
Quaternion<TYPE> lerp(const TYPE t, const Quaternion<TYPE>& a, const Quaternion<TYPE>& b)
{
	return (a*(1.0-t) + b*t).normalize();
}

/**
 * Spherical interpolation.
 * http://willperone.net/Code/quaternion.php
 * http://w3.impa.br/~aschulz/anim/rotations.pdf
 * http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/index.html
 */
template <typename TYPE>
Quaternion<TYPE> slerp(const TYPE t, const Quaternion<TYPE>& a, const Quaternion<TYPE>& b)
{
	Quaternion<TYPE> ret;
	TYPE dot = a.dot(b);

	// dot = cos(theta)
	// If (dot < 0), a and b are more than 90 degrees apart,
	// So we can invert one to reduce spinning.
	if (dot < 0.0)
	{
		dot = -dot;
		ret = -b;
	}
	else
		ret = b;

	if (dot < 0.95)
	{
		CLAMP(dot, -1.0, 1.0);
		TYPE theta  = acos(dot);
		return (( (a * sin(theta*(1.0-t))) + (ret * sin(theta*t)) / sin(theta))).normalize();
	}
	else
	{
		// If the angle is small, use linear interpolation.
		return lerp(t, a, b); // Lerp normalizes quaternion by itself.
	}
}


template <typename TYPE>
Quaternion<TYPE> operator+(const Quaternion<TYPE>& a, const Quaternion<TYPE>& b)
{
	return Quaternion<TYPE>(a.w + b.w, a.x + b.x, a.y + b.y, a.z + b.z);
}

INSTANTIATE_VECTOR_TEMPLATE(Quaternion);

template Quaternion<float>	operator+(const Quaternion<float>&  a, const Quaternion<float>&  b);
template Quaternion<double>	operator+(const Quaternion<double>& a, const Quaternion<double>& b);
template Quaternion<double> lerp (const double t, const Quaternion<double>& a, const Quaternion<double>& b);
template Quaternion<float> 	slerp(const float  t, const Quaternion<float>&  a, const Quaternion<float>&  b);
template Quaternion<double> slerp(const double t, const Quaternion<double>& a, const Quaternion<double>& b);

